/*
 * communication.cpp
 *
 *  Created on: Apr 26, 2017
 *      Author: zeroun
 */

#include "communication/communication.h"
#include <ros/console.h>
#include <fnmatch.h>

#ifdef MAVLINK_VERSION
#undef MAVLINK_VERSION
#endif
#include <mavlink/config.h>

using namespace communication;
using namespace mavconn;
using namespace commplugin;


Communication::Communication() :
    mavlink_nh("mavlink"),		// allow to namespace it
    nh("~"),
    nodeAlive(0),
	plugin_loader("communication", "commplugin::CommPlugin"),
	message_route_table {}
{
    std::string station_url;
    int system_id, component_id;
	ros::V_string plugin_blacklist{}, plugin_whitelist{};



    //insert the mmsg id need to send to fcu directly
    mmsgToFCU.insert(11);
    mmsgToFCU.insert(20);
    mmsgToFCU.insert(21);
    mmsgToFCU.insert(23);
    mmsgToFCU.insert(76);
    mmsgToFCU.insert(41);
    mmsgToFCU.insert(75);
    //insert the mmsg id need to send to gcs directly
    mmsgToGCS.insert(0);
    mmsgToGCS.insert(1);
    mmsgToGCS.insert(24);
    mmsgToGCS.insert(30);
    mmsgToGCS.insert(33);
    mmsgToGCS.insert(74);
    mmsgToGCS.insert(22);
    mmsgToGCS.insert(77);
    mmsgToGCS.insert(253);
    mmsgToGCS.insert(148);


    float cmd_close_flt;
    nh.param<std::string>("station_url", station_url, "serial:///dev/ttyUSB1:115200");
    nh.param("system_id", system_id, 6);
    nh.param<int>("component_id", component_id, 1);
    nh.param("target_system_id", tgt_system_id, 0);
    nh.param("target_component_id", tgt_component_id, 1);
    nh.getParam("/CMD_CLOSE_FLT",cmd_close_flt);
	nh.getParam("plugin_blacklist", plugin_blacklist);
	nh.getParam("plugin_whitelist", plugin_whitelist);

    if(cmd_close_flt)
    {
        closedFilter = true;
    }else
    {
        closedFilter = false;
    }


    ROS_INFO_STREAM("STATION URL: " << station_url);
	try {
        station_link = augmavconn::AugMavConnInterface::open_url(station_url, system_id, component_id);
		// may be overridden by URL
        system_id = station_link->get_system_id();
        component_id = station_link->get_component_id();


	}
	catch (mavconn::DeviceError &ex) {
        ROS_FATAL("STATION: %s", ex.what());
		ros::shutdown();
		return;
	}

	// ROS mavlink bridge
    mavlink_pub = mavlink_nh.advertise<mavros_msgs::Mavlink>("to", 100);
    mavlink_sub = mavlink_nh.subscribe("from", 100, &Communication::mavlink_sub_cb, this,
		ros::TransportHints()
			.unreliable().maxDatagramSize(1024)
			.reliable());

    // connect STATION link
    station_link->message_received.connect(boost::bind(&Communication::mavlink_pub_cb, this, _1, _2, _3));
    /*fcu_link->message_received.connect([this](const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid){
        if(this->mmsgToFCU.count(mmsg->msgid))
			this->mavlink_pub_cb(mmsg,sysid,compid);
    });*/


    //station_link->message_received.connect(boost::bind(&Communication::plugin_route_cb, this, _1, _2, _3));
    station_link->message_received.connect(boost::bind(&Communication::plugin_route_cb, this, _1, _2, _3));
    station_link->port_closed.connect(boost::bind(&Communication::terminate_cb, this));


	// prepare plugin lists
	// issue #257 2: assume that all plugins blacklisted
	if (plugin_blacklist.empty() and !plugin_whitelist.empty())
		plugin_blacklist.push_back("*");

    //ROS_INFO("size=%d",plugin_loader.getDeclaredClasses().size());

 //   if(!closedFilter)
//    {
        for (auto &name : plugin_loader.getDeclaredClasses())
            add_plugin(name, plugin_blacklist, plugin_whitelist);
//    }

        //hostHeartbeat_sub = nh.subscribe("/commander/heart_beat",1, &Communication::ReceiveHostHeartb, this);
        pos_cog_sub = nh.subscribe("/mavros/global_position/raw/cog", 1, &Communication::set_cog, this);
        receiveHParamSrv = nh.advertiseService("/communication/host_param/set_param",&Communication::receiveHParamSetSrv,this);
        //用于检测倾转是否转为固定翼
        vtolStateClient = nh.serviceClient<smarteye_common::UInt16Srv>("/commander/vtol_state");

#define STR2(x)	#x
#define STR(x)	STR2(x)

	ROS_INFO("Built-in MAVLink package version: %s", MAVLINK_VERSION);
	ROS_INFO("Built-in MAVLink dialect: %s", STR(MAVLINK_DIALECT));
    //ROS_INFO("Communication started. MY ID %d.%d, TARGET ID %d.%d",
    //	system_id, component_id,
    //	tgt_system_id, tgt_component_id);
    ROS_INFO("Communication closedFilter == %d",closedFilter);

}

void Communication::spin() {
	ros::AsyncSpinner spinner(4 /* threads */);


	spinner.start();
	ros::waitForShutdown();

    ROS_INFO("Stopping Communication...");
	spinner.stop();
}

void Communication::mavlink_pub_cb(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid) {


    if(mmsg->msgid == 21)
    {
        mavlink_param_request_list_t plistMsg;
        mavlink_msg_param_request_list_decode(mmsg,&plistMsg);
        // ROS_DEBUG("pub 21, tgt_com:%d, my_com:%d",(int)plistMsg.target_component,station_link->get_component_id());
        if (plistMsg.target_system != station_link->get_system_id()
                || (plistMsg.target_system == station_link->get_system_id()
                    && plistMsg.target_component == station_link->get_component_id()))
        {
            return;
        }
    }else if(mmsg->msgid == 20)
    {
        mavlink_param_request_read_t prMsg;
        mavlink_msg_param_request_read_decode(mmsg,&prMsg);
        if (prMsg.target_system != station_link->get_system_id()
                ||(prMsg.target_system == station_link->get_system_id()
                   && prMsg.target_component == station_link->get_component_id()))
        {
            return;
        }
    }else if(mmsg->msgid == 23)
    {
        mavlink_param_set_t psMsg;
        mavlink_msg_param_set_decode(mmsg, &psMsg);
        if (psMsg.target_system != station_link->get_system_id()
                ||(psMsg.target_system == station_link->get_system_id()
                   && psMsg.target_component == station_link->get_component_id()))
        {
            return;
        }
    }else if(mmsg->msgid == MAVLINK_MSG_ID_SET_MODE)
    {
        mavlink_set_mode_t smMsg;
        mavlink_msg_set_mode_decode(mmsg, &smMsg);
        if (smMsg.target_system != station_link->get_system_id())
        {
            return;
        }
    }else if(mmsg->msgid == MAVLINK_MSG_ID_MISSION_SET_CURRENT)
    {
        mavlink_mission_set_current_t mscMsg;
        mavlink_msg_mission_set_current_decode(mmsg, &mscMsg);
        if (mscMsg.target_system != station_link->get_system_id())
        {
            return;
        }
    }else if(mmsg->msgid == MAVLINK_MSG_ID_COMMAND_LONG)
    {
        mavlink_command_long_t clMsg;
        mavlink_msg_command_long_decode(mmsg,&clMsg);
        if (clMsg.target_system != station_link->get_system_id()
                ||(clMsg.target_system == station_link->get_system_id()
                   && clMsg.target_component == station_link->get_component_id()))
        {
            return;

        }
    }else if(mmsg->msgid == MAVLINK_MSG_ID_COMMAND_INT)
    {
        mavlink_command_int_t ciMsg;
        mavlink_msg_command_int_decode(mmsg,&ciMsg);
        if (ciMsg.target_system != station_link->get_system_id()
                ||(ciMsg.target_system == station_link->get_system_id()
                   && ciMsg.target_component == station_link->get_component_id()))
        {
            return;

        }
    }


    bool sendMsg = false;
    //ROS_INFO("closedFilter=%b",closedFilter);

    if(closedFilter && mmsg->msgid != 47 && mmsg->msgid != 44 && mmsg->msgid != 43 && mmsg->msgid != 40 && mmsg->msgid != 39)
    {
        sendMsg = true;
    }
    else if(mmsgToFCU.count(mmsg->msgid))
    {
        sendMsg = true;
    }

    if(sendMsg == true)
    {
        auto rmsg = boost::make_shared<mavros_msgs::Mavlink>();
        if  (mavlink_pub.getNumSubscribers() == 0)
            return;
        rmsg->header.stamp = ros::Time::now();
        mavros_msgs::mavlink::convert(*mmsg, *rmsg);
        mavlink_pub.publish(rmsg);
        //ROS_INFO("mmsg to fcu : %d",mmsg->msgid);
    }

}

void Communication::mavlink_sub_cb(const mavros_msgs::Mavlink::ConstPtr &rmsg) {
    //    bool sendMsg = false;
    //    if(closedFilter)
    //    {
    //        sendMsg = true;
    //    }
    //    else if(mmsgToGCS.count(rmsg->msgid)){
    //        sendMsg = true;
    //    }

    //    if(sendMsg == true)
    //    {

//    if(rmsg->msgid == 22)
//    {
//        mavlink_message_t mmsg_tmp;
//        if (mavros_msgs::mavlink::convert(*rmsg, mmsg_tmp))
//        {
//            mavlink_param_value_t sys_status;

//            mavlink_msg_param_value_decode(&mmsg_tmp, &sys_status);
//            std::string param_id = sys_status.param_id;
//            if(!param_id.compare("SYS_AUTOSTART"))
//            {
//                ROS_ERROR("SYS_AUTOSTART");
//                ROS_ERROR("value=%f, type=%u, count=%u, index=%u",sys_status.param_value,sys_status.param_type,sys_status.param_count,sys_status.param_index);

//            }
//        }

//    }

    if(rmsg->msgid != 33 && rmsg->msgid != 74 && rmsg->msgid != 24 && rmsg->msgid != 30 && rmsg->msgid != 1)
    {
        // ROS_DEBUG("mmsg to gcs : %d",rmsg->msgid);
    }
    if(rmsg->msgid == 0)//heartbeat
    {
        if(station_link->get_system_id()!=rmsg->sysid)
        {
            station_link->set_system_id(rmsg->sysid);
            nh.setParam("/communication/system_id",rmsg->sysid);
        }

        // ROS_DEBUG("sysid = %d",(int)rmsg->sysid);

        mavlink_message_t mmsg;
        if (mavros_msgs::mavlink::convert(*rmsg, mmsg))
            station_link->send_message(&mmsg, rmsg->sysid, rmsg->compid);
        else
            ROS_ERROR("Drop mavlink packet: illegal payload64 size");

    }else if(rmsg->msgid == 1)
    {
        // ROS_DEBUG("sysid = %d",(int)rmsg->sysid);
        mavlink_message_t mmsg_tmp;
        if (mavros_msgs::mavlink::convert(*rmsg, mmsg_tmp))
        {
            mavlink_sys_status_t sys_status;
            mavlink_msg_sys_status_decode(&mmsg_tmp, &sys_status);
            if(sys_status.errors_count1==1)
            {
                
                smarteye_common::UInt16Srv srv;
                srv.request.data=1; 
                if (vtolStateClient.call(srv))
                {
                    if(srv.response.success)
                        ROS_DEBUG("comm: Successed to call service vtol_state");
                    else
                        ROS_ERROR("comm: Service vtol_state return false");
                }
                else
                {
                    ROS_ERROR("comm:Failed to call service vtol_state");
                }
            }
            station_link->send_message(&mmsg_tmp);
        }
        else
            ROS_ERROR("comm: Drop mavlink packet: illegal payload64 size");
    }
    else if(rmsg->msgid == 33)
    {
        mavlink_message_t mmsg_tmp;
        if (mavros_msgs::mavlink::convert(*rmsg, mmsg_tmp))
        {
            mavlink_global_position_int_t gpos;
            mavlink_msg_global_position_int_decode(&mmsg_tmp, &gpos);
            gpos.hdg = pos_cog;
            mavlink_msg_global_position_int_encode(station_link->get_system_id(),
                                                   station_link->get_component_id(),
                                                   &mmsg_tmp,
                                                   &gpos);
            station_link->send_message(&mmsg_tmp);
        }
        else
            ROS_ERROR("Drop mavlink Global position int packet: illegal payload64 size");
    }else
    {
        mavlink_message_t mmsg;
        if (mavros_msgs::mavlink::convert(*rmsg, mmsg))
            station_link->send_message(&mmsg, rmsg->sysid, rmsg->compid);
        else
            ROS_ERROR("Drop mavlink packet: illegal payload64 size");
    }

}

void Communication::plugin_route_cb(const mavlink_message_t *mmsg, uint8_t sysid, uint8_t compid) {
        message_route_table[mmsg->msgid](mmsg, sysid, compid);
}

static bool pattern_match(std::string &pattern, std::string &pl_name) {
	int cmp = fnmatch(pattern.c_str(), pl_name.c_str(), FNM_CASEFOLD);
	if (cmp == 0)
		return true;
	else if (cmp != FNM_NOMATCH) {
		// never see that, i think that it is fatal error.
		ROS_FATAL("Plugin list check error! fnmatch('%s', '%s', FNM_CASEFOLD) -> %d",
				pattern.c_str(), pl_name.c_str(), cmp);
		ros::shutdown();
	}

	return false;
}

/**
 * @brief Checks that plugin blacklisted
 *
 * Operation algo:
 *
 *  1. if blacklist and whitelist is empty: load all
 *  2. if blacklist is empty and whitelist non empty: assume blacklist is ["*"]
 *  3. if blacklist non empty: usual blacklist behavior
 *  4. if whitelist non empty: override blacklist
 *
 * @note Issue #257.
 */
bool Communication::is_blacklisted(std::string &pl_name, ros::V_string &blacklist, ros::V_string &whitelist) {
	for (auto &bl_pattern : blacklist) {
		if (pattern_match(bl_pattern, pl_name)) {
			for (auto &wl_pattern : whitelist) {
				if (pattern_match(wl_pattern, pl_name))
					return false;
			}

			return true;
		}
	}

	return false;
}

/**
 * @brief Loads plugin (if not blacklisted)
 */
void Communication::add_plugin(std::string &pl_name, ros::V_string &blacklist, ros::V_string &whitelist) {
	if (is_blacklisted(pl_name, blacklist, whitelist)) {
		ROS_INFO_STREAM("Plugin " << pl_name << " blacklisted");
		return;
	}

	try {
		auto plugin = plugin_loader.createInstance(pl_name);
        plugin->initialize(station_link, tgt_system_id, tgt_component_id);
		loaded_plugins.push_back(plugin);
		ROS_INFO_STREAM("Plugin " << pl_name << " loaded and initialized");

		for (auto &pair : plugin->get_rx_handlers()) {
			ROS_DEBUG_STREAM("Route msgid " << int(pair.first) << " to " << pl_name);
			message_route_table[pair.first].connect(pair.second);
		}
	} catch (pluginlib::PluginlibException &ex) {
		ROS_ERROR_STREAM("Plugin " << pl_name << " load exception: " << ex.what());
	}
}

void Communication::terminate_cb() {
    ROS_ERROR("STATION connection closed, Communication will be terminated.");
	ros::requestShutdown();
}

void Communication::ReceiveHostHeartb(const std_msgs::UInt8& msg)
{
    unique_lock lock(mutex);
    nodeAlive = msg.data;
    lock.unlock();
}

void Communication::set_cog(const std_msgs::Float32::ConstPtr& cog)
{
    unique_lock lock(mutex);
    pos_cog = cog->data * 1E2 ;
    isCogSet = true;
    lock.unlock();
}

bool Communication::receiveHParamSetSrv(smarteye_common::HParamSetSrv::Request &req, smarteye_common::HParamSetSrv::Response &res)
{
    ROS_DEBUG("Communication received new Param: name: %s value: %f",req.param_id.c_str(),req.param_value);
    unique_lock lock(mutex);
    if(req.param_id == "CMD_CLOSE_FLT")
    {
        if(req.param_value)
            closedFilter = true;
        else
            closedFilter = false;

        res.paramset_ack = true;
    }
    else
    {
        res.paramset_ack = false;
    }
    lock.unlock();
    return true;
}




